
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pi_2d.h
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdlib.h>
#include <stdbool.h>

#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define PI_2D_FILT_HZ_DEFAULT  20.0f   // default input filter frequency
#define PI_2D_FILT_HZ_MIN      0.01f   // minimum input filter frequency
/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    float _kp;
    float _ki;
    float _kimax;
    float _filt_hz;    // PID Input filter frequency in Hz

    bool _flags_reset_filter;

    // internal variables
    float      dt;                // timestep in seconds
    Vector2f_t integrator;        // integrator value
    Vector2f_t input;             // last input for derivative
    float      filt_alpha;        // input filter alpha
} PI_2d_ctrl;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void pi_2d_ctrl_init(PI_2d_ctrl * controler, float kp, float ki, float kimax, float filt_hz, float initial_dt);

// set_dt - set time step in seconds
void pi_2d_ctrl_set_dt(PI_2d_ctrl * controler, float dt);

// set_input - set input to PID controller
//  input is filtered before the PID controllers are run
//  this should be called before any other calls to get_p, get_i or get_d
void pi_2d_ctrl_set_input(PI_2d_ctrl * controler, Vector2f_t input);

// get_pi - get results from pid controller
Vector2f_t pi_2d_ctrl_get_pi(PI_2d_ctrl * controler);
Vector2f_t pi_2d_ctrl_get_p(PI_2d_ctrl * controler);
Vector2f_t pi_2d_ctrl_get_i(PI_2d_ctrl * controler);
Vector2f_t pi_2d_ctrl_get_i_shrink(PI_2d_ctrl * controler);   // get_i but do not allow integrator to grow (it may shrink)

// reset_I - reset the integrator
void pi_2d_ctrl_reset_I(PI_2d_ctrl * controler);

// reset_filter - input and D term filter will be reset to the next value provided to set_input()
static inline void pi_2d_ctrl_reset_filter(PI_2d_ctrl * controler) { controler->_flags_reset_filter = true; }

// set accessors
static inline void pi_2d_ctrl_set_kp(PI_2d_ctrl * controler, float kp) { controler->_kp = kp; }
static inline void pi_2d_ctrl_set_ki(PI_2d_ctrl * controler, float ki) { controler->_ki = ki; }
static inline void pi_2d_ctrl_set_kimax(PI_2d_ctrl * controler, float kimax) { controler->_kimax = fabsf(kimax); }
static inline void pi_2d_ctrl_set_filt_hz(PI_2d_ctrl * controler, float filt_hz);

static inline float pi_2d_ctrl_get_kp(PI_2d_ctrl * controler) { return controler->_kp; }
static inline float pi_2d_ctrl_get_ki(PI_2d_ctrl * controler) { return controler->_ki; }
static inline float pi_2d_ctrl_get_kimax(PI_2d_ctrl * controler) { return controler->_kimax; }
static inline float pi_2d_ctrl_get_filt_hz(PI_2d_ctrl * controler) { return controler->_filt_hz; }
static inline float pi_2d_ctrl_get_filt_alpha(PI_2d_ctrl * controler) { return controler->filt_alpha; }

static inline Vector2f_t pi_2d_ctrl_get_integrator(PI_2d_ctrl * controler) { return controler->integrator; }
static inline void pi_2d_ctrl_set_integrator2d(PI_2d_ctrl * controler, Vector2f_t i) { controler->integrator = i; }
void pi_2d_ctrl_set_integrator3d(PI_2d_ctrl * controler, Vector3f_t i);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



